# 机器人控制话题说明
# | 功能       | 话题名           | 消息类型                  |
# | ---------- | ---------------- | ------------------------- |
# | 控制小车   | /cmd_vel         | geometry_msgs/msg/Twist   |
# | 控制蜂鸣器 | /buzzer          | std_msgs/msg/UInt16       |
# | 控制舵机S1 | /servo1_angle    | std_msgs/msg/Int32        |
# | 控制舵机S2 | /servo2_angle    | std_msgs/msg/Int32        |
#
# 备注：
# 1. 控制蜂鸣的data数值大小[0, 10000]，当data=0时蜂鸣器关闭。当data=1时，蜂鸣器一直鸣笛。当data>=10时，蜂鸣器响data毫秒后自动关闭。
# 2. 控制舵机S1的data数值大小[-90, 90]。
# 3. 控制舵机S2的data数值大小[-90, 20]。
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import UInt16, Int32

node = None
pub_cmd_vel = None
pub_beep = None
pub_servo1 = None
pub_servo2 = None

def init_publishers():
    global node, pub_cmd_vel, pub_beep, pub_servo1, pub_servo2
    rclpy.init(args=None)
    node = rclpy.create_node('robot_control')
    pub_cmd_vel = node.create_publisher(Twist, '/cmd_vel', 10)
    pub_beep = node.create_publisher(UInt16, '/buzzer', 10)
    pub_servo1 = node.create_publisher(Int32, '/servo1_angle', 10)
    pub_servo2 = node.create_publisher(Int32, '/servo2_angle', 10)

def cmd_vel(linear=0.0, angular=0.0):
    global node, pub_cmd_vel
    msg = Twist()
    msg.linear.x = linear
    msg.angular.z = angular
    pub_cmd_vel.publish(msg)
    rclpy.spin_once(node, timeout_sec=0.1)

def beep(state: int):
    global node, pub_beep
    msg = UInt16()
    msg.data = state
    pub_beep.publish(msg)
    rclpy.spin_once(node, timeout_sec=0.1)

def servo1(node, pub, angle: int):
    """
    发布控制舵机S1的Int32消息。
    angle: 舵机角度，范围[-90, 90]
    """
    angle = max(-90, min(90, angle))
    msg = Int32()
    msg.data = angle
    pub.publish(msg)
    rclpy.spin_once(node, timeout_sec=0.1)

def servo2(node, pub, angle: int):
    """
    发布控制舵机S2的Int32消息。
    angle: 舵机角度，范围[-90, 20]
    """
    angle = max(-90, min(20, angle))
    msg = Int32()
    msg.data = angle
    pub.publish(msg)
    rclpy.spin_once(node, timeout_sec=0.1)